{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import glob\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "\n",
    "objp = np.zeros((6*7,3), np.float32)#准备像素点\n",
    "objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)#标记像素点\n",
    "\n",
    "objpoints = [] # 3d 坐标\n",
    "imgpoints = [] # 2d 坐标\n",
    "\n",
    "images = glob.glob(r'C:\\Users\\ASUS\\Desktop\\camera\\*.jpg')\n",
    "\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (7,6),None)#找到角点\n",
    "\n",
    "    if ret == True:\n",
    "        objpoints.append(objp)\n",
    "\n",
    "        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)\n",
    "        imgpoints.append(corners2)#添加角点\n",
    "\n",
    "        img = cv2.drawChessboardCorners(img, (7,6), corners2,ret)\n",
    "        cv2.imshow('img',img)\n",
    "        cv2.waitKey(500)\n",
    "        cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)#相机标定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "type objpoints:[[0. 0. 0.]\n",
      " [1. 0. 0.]\n",
      " [2. 0. 0.]\n",
      " [3. 0. 0.]\n",
      " [4. 0. 0.]\n",
      " [5. 0. 0.]\n",
      " [6. 0. 0.]\n",
      " [0. 1. 0.]\n",
      " [1. 1. 0.]\n",
      " [2. 1. 0.]\n",
      " [3. 1. 0.]\n",
      " [4. 1. 0.]\n",
      " [5. 1. 0.]\n",
      " [6. 1. 0.]\n",
      " [0. 2. 0.]\n",
      " [1. 2. 0.]\n",
      " [2. 2. 0.]\n",
      " [3. 2. 0.]\n",
      " [4. 2. 0.]\n",
      " [5. 2. 0.]\n",
      " [6. 2. 0.]\n",
      " [0. 3. 0.]\n",
      " [1. 3. 0.]\n",
      " [2. 3. 0.]\n",
      " [3. 3. 0.]\n",
      " [4. 3. 0.]\n",
      " [5. 3. 0.]\n",
      " [6. 3. 0.]\n",
      " [0. 4. 0.]\n",
      " [1. 4. 0.]\n",
      " [2. 4. 0.]\n",
      " [3. 4. 0.]\n",
      " [4. 4. 0.]\n",
      " [5. 4. 0.]\n",
      " [6. 4. 0.]\n",
      " [0. 5. 0.]\n",
      " [1. 5. 0.]\n",
      " [2. 5. 0.]\n",
      " [3. 5. 0.]\n",
      " [4. 5. 0.]\n",
      " [5. 5. 0.]\n",
      " [6. 5. 0.]]\n",
      "type imgpoints:[[[475.32184  264.62466 ]]\n",
      "\n",
      " [[440.50244  263.23285 ]]\n",
      "\n",
      " [[406.2218   261.7014  ]]\n",
      "\n",
      " [[372.6837   259.91016 ]]\n",
      "\n",
      " [[340.0107   258.24222 ]]\n",
      "\n",
      " [[308.49207  256.51584 ]]\n",
      "\n",
      " [[277.5959   255.09286 ]]\n",
      "\n",
      " [[476.69138  230.0038  ]]\n",
      "\n",
      " [[441.2503   228.63284 ]]\n",
      "\n",
      " [[406.59464  227.64816 ]]\n",
      "\n",
      " [[372.70612  226.32228 ]]\n",
      "\n",
      " [[339.55396  225.40205 ]]\n",
      "\n",
      " [[307.56766  224.25938 ]]\n",
      "\n",
      " [[276.92807  223.40599 ]]\n",
      "\n",
      " [[477.408    194.33427 ]]\n",
      "\n",
      " [[441.71268  193.62064 ]]\n",
      "\n",
      " [[406.76755  192.52245 ]]\n",
      "\n",
      " [[372.57828  192.05171 ]]\n",
      "\n",
      " [[339.26407  191.56076 ]]\n",
      "\n",
      " [[307.0833   191.06427 ]]\n",
      "\n",
      " [[275.86005  190.52164 ]]\n",
      "\n",
      " [[477.91467  158.32228 ]]\n",
      "\n",
      " [[442.11322  157.88603 ]]\n",
      "\n",
      " [[406.80106  157.49673 ]]\n",
      "\n",
      " [[372.3857   157.41675 ]]\n",
      "\n",
      " [[338.89185  157.39815 ]]\n",
      "\n",
      " [[306.548    157.64893 ]]\n",
      "\n",
      " [[275.25006  158.04948 ]]\n",
      "\n",
      " [[478.01114  122.23826 ]]\n",
      "\n",
      " [[442.09683  122.08506 ]]\n",
      "\n",
      " [[406.66895  122.14299 ]]\n",
      "\n",
      " [[372.24942  122.28639 ]]\n",
      "\n",
      " [[338.62314  123.086105]]\n",
      "\n",
      " [[306.05914  123.96403 ]]\n",
      "\n",
      " [[274.70535  124.87432 ]]\n",
      "\n",
      " [[477.62332   86.22188 ]]\n",
      "\n",
      " [[441.63654   86.24669 ]]\n",
      "\n",
      " [[406.45435   86.711365]]\n",
      "\n",
      " [[371.72195   87.874794]]\n",
      "\n",
      " [[338.3092    88.79298 ]]\n",
      "\n",
      " [[305.50098   90.3172  ]]\n",
      "\n",
      " [[274.3947    92.21057 ]]]\n",
      "mtx外参:\n",
      " [[534.01662034   0.         334.81844055]\n",
      " [  0.         533.49965109 238.64878552]\n",
      " [  0.           0.           1.        ]]\n",
      "dist畸变值:\n",
      " [[-0.31437109  0.26521916  0.00118447 -0.00106879 -0.27419239]]\n",
      "x旋转（向量）外参:\n",
      " [array([[-0.45356791],\n",
      "       [ 0.26633136],\n",
      "       [-3.08336673]]), array([[ 0.41482424],\n",
      "       [ 0.67341033],\n",
      "       [-1.33827263]]), array([[-0.28036796],\n",
      "       [-0.37625374],\n",
      "       [-2.74629815]]), array([[-0.39982042],\n",
      "       [-0.16544825],\n",
      "       [-3.11223624]]), array([[-0.4639848 ],\n",
      "       [-0.29491077],\n",
      "       [-1.75878726]]), array([[-0.29871406],\n",
      "       [ 0.40680068],\n",
      "       [-1.43318573]]), array([[-0.31960901],\n",
      "       [ 0.17976131],\n",
      "       [-1.23762622]]), array([[-0.41407605],\n",
      "       [ 0.24520159],\n",
      "       [-3.097198  ]]), array([[-0.25720109],\n",
      "       [-0.40221215],\n",
      "       [-2.75533752]]), array([[0.36141943],\n",
      "       [0.19248598],\n",
      "       [3.10471858]]), array([[-0.30374818],\n",
      "       [ 0.37598395],\n",
      "       [-1.44073875]]), array([[-0.3200787 ],\n",
      "       [ 0.14780088],\n",
      "       [-1.24994034]])]\n",
      "dist平移（向量）外参:\n",
      " [array([[ 3.98177323],\n",
      "       [ 0.74003255],\n",
      "       [14.78822341]]), array([[-1.96118241],\n",
      "       [ 1.71064623],\n",
      "       [12.84485294]]), array([[3.16260423],\n",
      "       [2.63761611],\n",
      "       [9.86847861]]), array([[ 2.96046511],\n",
      "       [ 2.10523148],\n",
      "       [10.91905762]]), array([[-1.01780512],\n",
      "       [ 2.57198536],\n",
      "       [ 9.59000301]]), array([[ 1.80981517],\n",
      "       [ 3.64199053],\n",
      "       [16.10319711]]), array([[-5.72609601],\n",
      "       [ 2.21399485],\n",
      "       [16.85273752]]), array([[ 0.313333  ],\n",
      "       [ 1.07570496],\n",
      "       [14.72920993]]), array([[-0.39484322],\n",
      "       [ 2.87767939],\n",
      "       [ 9.78893294]]), array([[-0.61795509],\n",
      "       [ 2.36481739],\n",
      "       [10.84582446]]), array([[-1.89842807],\n",
      "       [ 4.00919616],\n",
      "       [15.93483343]]), array([[-9.45386834],\n",
      "       [ 2.64479769],\n",
      "       [16.58301674]])]\n"
     ]
    }
   ],
   "source": [
    "print(f\"type objpoints:{objpoints[0]}\")\n",
    "print(f\"type imgpoints:{imgpoints[0]}\")\n",
    "print (\"mtx外参:\\n\",mtx)\n",
    "print (\"dist畸变值:\\n\",dist)\n",
    "print (\"x旋转（向量）外参:\\n\",rvecs)\n",
    "print (\"dist平移（向量）外参:\\n\",tvecs)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "f/dx,f/dy未到千级，分析可能是使用的为opencv内部的资料图片，此图片的相机焦距较短。但主点接近拍摄宽、高的一半位置。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "total error:  0.02665649057099717\n"
     ]
    }
   ],
   "source": [
    "tot_error = 0\n",
    "for i in range(len(objpoints)):\n",
    "    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)\n",
    "    error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)\n",
    "    tot_error += error\n",
    "\n",
    "print (\"total error: \", tot_error/len(objpoints))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "误差较小。定位较好"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[6. 2. 0.]\n",
      " [0. 3. 0.]\n",
      " [1. 3. 0.]\n",
      " [2. 3. 0.]]\n",
      "[[[275.86005 190.52164]]\n",
      "\n",
      " [[477.91467 158.32228]]\n",
      "\n",
      " [[442.11322 157.88603]]\n",
      "\n",
      " [[406.80106 157.49673]]]\n"
     ]
    }
   ],
   "source": [
    "objp2=objpoints[0][20:24]#取4个特征点进行估计\n",
    "imgp2=imgpoints[0][20:24]\n",
    "\n",
    "print(objp2)\n",
    "print(imgp2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [],
   "source": [
    "retval2,rvec2,tvec2 = cv2.solvePnP(objp2,imgp2,mtx,dist)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {},
   "outputs": [],
   "source": [
    "rotM = cv2.Rodrigues(rvec2)[0]\n",
    "\n",
    "position = -np.matrix(rotM).T * np.matrix(tvec2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.004202213323145012 0.0001787932068056059 -0.010095308019042732\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(str(position[0,0]) + ' ' + str(position[1,0])+ ' ' + str(position[2,0])+ '\\n')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "得到的是世界坐标x，y以及高程z，可能因为选取源图像的原因，计算结果数值与原图不符，经观察原图的世界坐标系取向各异，故没有得到合适的相机位置。。。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
